//
// Created by zhangzhiwen on 25-5-23.
//

#include "User_main.h"
#include "OLED.h"
#include "stdio.h"
#include "tim.h"
//#define gy33_ColorIdentify() 0

int8_t Color_Record[2] = {-1,-1};  // 颜色记录数组
int8_t mode = MODE_IDLE;
int8_t mode_last = MODE_IDLE;
extern uint16_t last_step;

void User_hit(){            //the whole logic of hitting
    HAL_Delay(2000);
    buzzer_play(1000,1);
    SG90_hit();
}

void User_Init(){
    uint8_t config = 0x50;
    gy33_SendConfig(&config);
    encoder_init();
    motor_init();
    SG90_init();
    buzzer_off();
    SG90_set_angle(60);
}

void Start()
{
    HAL_Delay(500);
    __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
    SG90_set_angle(60);
    switch (mode){
        case MODE_IDLE:
        case MODE_COLOR_IDENTIFY:
            break;
        case MODE_HIT_2:
            HAL_Delay(2000);
            motor_move(STEP2);
            User_hit();
            break;
        case MODE_HIT_2_AND_4:
            HAL_Delay(2000);
            motor_move(STEP2);
            User_hit();

            HAL_Delay(2000);
            motor_move(STEP4);
            User_hit();
            break;
        case MODE_RECODE_AND_HIT_1:
            HAL_Delay(2000);
            motor_move(STEP1);
            HAL_Delay(1000);
            if(Color_Record[0] == gy33_ColorIdentify()){
                User_hit();
                break;
            }
            motor_move(STEP2);
            HAL_Delay(1000);
            if(Color_Record[0] == gy33_ColorIdentify()){
                User_hit();
                break;
            }

            motor_move(STEP3);
            HAL_Delay(1000);
            if(Color_Record[0] == gy33_ColorIdentify()){
                User_hit();
                break;
            }

            motor_move(STEP4);
            HAL_Delay(1000);
            if(Color_Record[0] == gy33_ColorIdentify()){
                User_hit();
                break;
            }
            break;
        case MODE_RECODE_AND_HIT_2:
            HAL_Delay(2000);
            uint16_t target =STEP1;
            uint16_t SecondTarget = 0;
            uint8_t FirstFlag = 0;

            while(1){
                if(FirstFlag == 1 && SecondTarget != 0){        //to find second
                    HAL_Delay(1000);
                    motor_move(SecondTarget);
                    HAL_Delay(1000);
                    User_hit();
                    break;
                }
                else if(FirstFlag == 1 && SecondTarget == 0){   //has found second,so to hit second
                    motor_move(target);
                    HAL_Delay(1000);
                    if(Color_Record[1] == gy33_ColorIdentify()){
                        User_hit();
                        break;
                    }
                }else{                                          //to find first and record second
                    motor_move(target);
                    HAL_Delay(1000);
                    if(Color_Record[0] == gy33_ColorIdentify()){
                        User_hit();
                        FirstFlag = 1;
                    }else if(Color_Record[1] == gy33_ColorIdentify()){
                        SecondTarget = target;
                    }
                }

                //renew the move target [MOVE FORWARD]
                if(target == STEP1)target = STEP2;
                else if(target == STEP2)target = STEP3;
                else if(target == STEP3)target = STEP4;
            }
            break;

    }
    motor_reinit();
    OLED_ShowString(0, 6, "       ", 12, 0);
    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
    SG90_set_angle(60);
}

void JudgeColors(){
    uint8_t color = gy33_ColorIdentify();
    if(mode == MODE_COLOR_IDENTIFY) {
        if (Color_Record[0] == -1) {
            Color_Record[0] = color;
        } else if (Color_Record[1] == -1) {
            Color_Record[1] = color;
        } else {
            Color_Record[0] = Color_Record[1];
            Color_Record[1] = color;
        }
        buzzer_play(200, 2);
    } else{
        buzzer_play(200,color+1);
    }
}

uint8_t KeyFlag = 0;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if(htim->Instance == TIM1)
    {
        KeyFlag = 1;
    }
}

void key_loop()
{
    if(KeyFlag == 1)
    {
        KeyFlag = 0;
        if(HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin) == GPIO_PIN_RESET)
        {
            mode = (mode+1) % 6;        //Mode Forward
        }
        else if(HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin) == GPIO_PIN_RESET)
        {
            mode = (mode+5) % 6;       //Mode Backward
        }
        else if(HAL_GPIO_ReadPin(KEY3_GPIO_Port, KEY3_Pin) == GPIO_PIN_RESET)
        {
            if(mode != MODE_IDLE){
                OLED_ShowString(0, 6, "Start!", 12, 0);
                Start();
            }               //Start
        }
        else if(HAL_GPIO_ReadPin(KEY4_GPIO_Port, KEY4_Pin) == GPIO_PIN_RESET)
        {
            JudgeColors();  //Identify Color
        }
        display_update();
    }
}

// void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
// {
//     if(GPIO_Pin == KEY1_Pin)
//     {
//         HAL_Delay(50);
//         while(HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin) == GPIO_PIN_RESET);
//         HAL_Delay(50);
//         mode = (mode+1) % 6;        //Mode Forward
//     }
//     else if(GPIO_Pin == KEY2_Pin)
//     {
//         HAL_Delay(50);
//         while(HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin) == GPIO_PIN_RESET);
//         HAL_Delay(50);
//         mode = (mode+5) % 6;       //Mode Backward
//     }
//     else if(GPIO_Pin == KEY3_Pin)
//     {
//         HAL_Delay(50);
//         while(HAL_GPIO_ReadPin(KEY3_GPIO_Port, KEY3_Pin) == GPIO_PIN_RESET);
//         HAL_Delay(50);
//         if(mode != MODE_IDLE){
//             OLED_ShowString(0, 6, "Start!", 12, 0);
//             Start();
//         }               //Start
//     }
//     else if(GPIO_Pin == KEY4_Pin)
//     {
//         HAL_Delay(50);
//         while(HAL_GPIO_ReadPin(KEY4_GPIO_Port, KEY4_Pin) == GPIO_PIN_RESET);
//         HAL_Delay(50);
//         JudgeColors();  //Identify Color
//     }
//     display_update();
// }

void display_update(){
    if (mode != mode_last) {
        mode_last = mode;
        OLED_Clear();
    }
    OLED_ShowString(0, MODE_IDLE, "IDLE--------------", 12, 0);
    OLED_ShowString(0, MODE_COLOR_IDENTIFY, "IDENTIFY----------", 12, 0);
    OLED_ShowString(0, MODE_HIT_2, "HIT_2-------------", 12, 0);
    OLED_ShowString(0, MODE_HIT_2_AND_4, "HIT_2_AND_4-------", 12, 0);
    OLED_ShowString(0, MODE_RECODE_AND_HIT_1, "RECODE_AND_HIT_1--", 12, 0);
    OLED_ShowString(0, MODE_RECODE_AND_HIT_2, "RECODE_AND_HIT_2--", 12, 0);
    OLED_ShowString(110, mode, "X", 12, 0);

    char Color1[7] = " ";
    char Color2[7] = " ";
    if(Color_Record[0]==0){
        sprintf(Color1,"Red   ");
    }else if(Color_Record[0]==1){
        sprintf(Color1,"Yellow");
    }else if(Color_Record[0]==2){
        sprintf(Color1,"Blue  ");
    }else if(Color_Record[0]==3){
        sprintf(Color1,"Green ");
    }else if(Color_Record[0]==4){
        sprintf(Color1,"Defaul");
    }

    if(Color_Record[1]==0){
        sprintf(Color2,"Red   ");
    }else if(Color_Record[1]==1){
        sprintf(Color2,"Yellow");
    }else if(Color_Record[1]==2){
        sprintf(Color2,"Blue  ");
    }else if(Color_Record[1]==3){
        sprintf(Color2,"Green ");
    }else if(Color_Record[1]==4){
        sprintf(Color1,"Defaul");
    }

    OLED_ShowString(0,7,Color1,12,0);
    OLED_ShowString(64,7,Color2,12,0);

}

